/**
  ******************************************************************************
  * File Name          : s6h4d_driver.cpp
  * Description       : s6h4d_driver program body
  ******************************************************************************
  *
  * Copyright (c) 2019 HopeMotion Co., Ltd. 
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without 
  * modification, are permitted, provided that the following conditions are met:
  *
  * 1. Redistribution of source code must retain the above copyright notice, 
  *    this list of conditions and the following disclaimer.
  * 2. Redistributions in binary form must reproduce the above copyright notice,
  *    this list of conditions and the following disclaimer in the documentation
  *    and/or other materials provided with the distribution.
  * 3. Neither the name of HopeMotion nor the names of other 
  *    contributors to this software may be used to endorse or promote products 
  *    derived from this software without specific written permission.
  * 4. This software, including modifications and/or derivative works of this 
  *    software, must execute solely and exclusively on microcontroller or
  *    microprocessor devices manufactured by or for HopeMotion.
  * 5. Redistribution and use of this software other than as permitted under 
  *    this license is void and will automatically terminate your rights under 
  *    this license. 
  *
  * THIS SOFTWARE IS PROVIDED BY HOPEMOTION AND CONTRIBUTORS "AS IS" 
  * AND ANY EXPRESS, IMPLIED OR STATUTORY WARRANTIES, INCLUDING, BUT NOT 
  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY, FITNESS FOR A 
  * PARTICULAR PURPOSE AND NON-INFRINGEMENT OF THIRD PARTY INTELLECTUAL PROPERTY
  * RIGHTS ARE DISCLAIMED TO THE FULLEST EXTENT PERMITTED BY LAW. IN NO EVENT 
  * SHALL HOPEMOTION OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
  * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, 
  * OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 
  * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 
  * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
  * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
  *
  ******************************************************************************
  */
/* Includes ------------------------------------------------------------------*/
#include <s6h4d_driver/s6h4d_driver.h>

std::string _port;
double rx_pos[3] = {0, 0, 0};
double rx_data[JOINT_MAX] = {0, 0, 0, 0, 0, 0};
double tx_data[JOINT_MAX] = {INIT_J1, INIT_J2, INIT_J3, INIT_J4, INIT_J5, INIT_J6};

double last[JOINT_MAX] = {0, 0, 0, 0, 0, 0};

unsigned char s6h4d_state = S6H4D_RESET;
unsigned char s6h4d_sync = 0;
/* Private function prototypes -----------------------------------------------*/

/**
  * @brief  接收s6h4d_msgs/Goal话题数据
  * @param  消息结构体指针
  * @retval 无
  */
void goalCB(const s6h4d_msgs::Goal::ConstPtr & msg)
{
  unsigned cmd = msg->cmd;
  last[0] = msg->a0;
  last[1] = msg->a1;
  last[2] = msg->a2;
  last[3] = msg->a3;
  last[4] = msg->a4;
  last[5] = msg->a5;

  if(
    (tx_data[0]==last[0])&&
    (tx_data[1]==last[1])&&
    (tx_data[2]==last[2])&&
    (tx_data[3]==last[3])&&
    (tx_data[4]==last[4])&&
    (tx_data[5]==last[5])
  )
  {
    //发送的角度相同，不用处理
  }
  else
  {
    tx_data[0] = last[0];
    tx_data[1] = last[1];
    tx_data[2] = last[2];
    tx_data[3] = last[3];
    tx_data[4] = last[4];
    tx_data[5] = last[5];
    
    printf("run joint:[%0.2f %0.2f %0.2f %0.2f %0.2f %0.2f]\r\n", 
        tx_data[0], tx_data[1], tx_data[2], tx_data[3], tx_data[4], tx_data[5]);

  switch(cmd)
  {
    case 2: //带加减速的直线插补定位
      s6h4d_txSig(2, tx_data);  
      break;
    case 3: //轴角度插补
      s6h4d_txSig(3, tx_data);  
      break;
  }
    
  }
}

/**
  * @brief  机械臂驱动主函数
  * @param
  * @retval
  */
int main(int argc, char**argv)
{
  ros::init(argc, argv, "s6h4d_driver");
  ros::NodeHandle nh;
  ros::Rate loop_rate(50);   //50hz = 20ms

   s6h4d_msgs::Feedback joint_fb;
  //返回关节实际角度
  ros::Publisher pub = nh.advertise<s6h4d_msgs::Feedback>("s6h4d_msgs/feedback", 100);

  //订阅角度话题
  ros::Subscriber sub_goal = nh.subscribe("s6h4d_msgs/goal", 100, goalCB);

  nh.getParam("/s6h4d_driver/port", _port);
  s6h4d_init(_port.c_str());

  while (ros::ok())
  {
    s6h4d_rxSig(rx_data, rx_pos);
    switch(s6h4d_state)
    {
      case S6H4D_RESET:
        if(s6h4d_sync>50)
        {
          last[0] = rx_data[J1];
          last[1] = rx_data[J2];
          last[2] = rx_data[J3];
          last[3] = rx_data[J4];
          last[4] = rx_data[J5];
          last[5] = rx_data[J6];
          printf("init joint:[ %0.2f %0.2f %0.2f %0.2f %0.2f %0.2f ]\r\n",
                      last[0],last[1],last[2],last[3],last[4],last[5]);
          s6h4d_state = S6H4D_PLAY;
        }
        else
        {
          s6h4d_sync++;
        }
        break;
      case S6H4D_PLAY:
        joint_fb.header.seq+=1;
        joint_fb.header.stamp = ros::Time::now();
		    joint_fb.header.frame_id = "s6h4d";

        joint_fb.shoulder_pan_joint= rx_data[J1];  
        joint_fb.shoulder_lift_joint = rx_data[J2]; 
        joint_fb.elbow_joint = rx_data[J3]; 
        joint_fb.wrist_1_joint = rx_data[J4]; 
        joint_fb.wrist_2_joint = rx_data[J5]; 
        joint_fb.wrist_3_joint = rx_data[J6]; 

        joint_fb.pose.X = rx_pos[POS_X];
        joint_fb.pose.Y = rx_pos[POS_Y];
        joint_fb.pose.Z = rx_pos[POS_Z];
    
        pub.publish(joint_fb);  
        break;
      case S6H4D_STOP:
        break;
      default:
        break;
    }
    ros::spinOnce();
    loop_rate.sleep();
  }

  s6h4d_deinit();

  return 0;
}
